#ifndef SD_DISCRETE_HPP
#define SD_DISCRETE_HPP

#include "config.hpp"
#include <algorithm>

namespace MT {

//! return the median of a list of numbers
//! the list is in-place sorted
template<typename _Scalar>
_Scalar median(_Scalar *values, int size)
{
    int N = size;
	if(!N) return 0;
    std::sort(values, values+N);
    if(N&1) return values[N/2];
    N /= 2;
    return (values[N-1]+values[N])/2;
}


//! round up to the next highest power of 2
inline uint32 ceilpow2(uint32 v)
{
    v--;
    v |= v >> 1;
    v |= v >> 2;
    v |= v >> 4;
    v |= v >> 8;
    v |= v >> 16;
    return v+1;
}

//! parity of a uint32
inline bool parity(uint32 v)
{
    v ^= v >> 16;
    v ^= v >> 8;
    v ^= v >> 4;
    return (0x6996 >> (v&0xf)) & 1;
}


//! argsort(X) = argsort of X's elements
//! TODO: generalize to matrix instead of vector,
//! probably taking care of both row-major and column-major matrices
template<typename _Scalar, int _Rows>
Eigen::Matrix<int, _Rows, 1> argsort(Eigen::Matrix<_Scalar, _Rows, 1> const &X, bool ascendingOrder=true)
{
    typedef _Scalar const *Ptr_Scalar;
    int N = X.rows(), i;
    std::vector<Ptr_Scalar> v(N); for(i = N; i--; ) v[i] = &X.coeff(i);

    struct comparer
    {
        static inline bool cmp(Ptr_Scalar id1, Ptr_Scalar id2) { return *id1 < *id2; }
    };
    if(ascendingOrder)
        std::sort(v.begin(), v.end(), &comparer::cmp);
    else
        std::sort(v.rbegin(), v.rend(), &comparer::cmp);
    Eigen::Matrix<int, _Rows, 1> id(N, 1);
    for(i = N; i--; ) id[i] = v[i]-X.data();
    return id;
}


} // namespace MT

#endif
